#include <SoftwareSerial.h>

/*
  Octorotor - Beta version
 Authors - Felipe Polido, Henrique Polido and Mehmet Ali 'Mali' Akmanalp
 */

#include <NewSoftSerial.h>

//DEFINE PINS
int ledPin = 13;
int pwmPin[6] = {
  3,5,6,9,10,11};
int rxPin = 7;
int txPin = 8;

//IMU Variables
float imuRow = 0;
float imuPit = 0;
float imuYaw = 0;

//Communication:
int header = 0;
boolean eStop = 0;
unsigned long timeOut = 0;

//Maintain time
unsigned long loopTime;

//Software Serial setup:
NewSoftSerial mySerial(rxPin,txPin);


void setup() {

  Serial.begin(57600); //
  pinMode(ledPin, OUTPUT);  
  for(int i = 0; i<=5;i++){
    pinMode(pwmPin[i], OUTPUT); 
  }

  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  mySerial.begin(9600);
}


void loop() {
  loopTime = 0;
  while(1){
    if(millis() > loopTime){
      loopTime = millis() + 20;
      //reset emergency stop
     
      //Read data from Joystick
      joyInput();
      //stopInput();
      //Read IMU
      requestIMU();
      //IMU request message for next iteration
      mySerial.print('a');
      //Print info to PC
      printtoPC();
      //Drive the Motors
      //autoControl();
      manualControl();
    }
  }
}






